/**
 * @file ebex_genetic_stats.c
 *
 * @date Oct 21, 2011
 * @author seth
 *
 * @brief This file is part of EBEX State Estimator, created for the EBEX project
 *
 * This software is copyright (C) 2011 Columbia University
 *
 * EBEX State Estimator is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * EBEX State Estimator is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with EBEX State Estimator; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include <ebex_vector.h>
#include <ebex_quaternion.h>

#include <ebex_genetic.h>
#include <ebex_sort.h>

#include <ebex_genetic_stats.h>


void genetic_allele_mean_quaternion(population_t *m_pop, size_t m_chromosome, size_t m_allele, ebex_quat_t *m_mean, size_t m_size)
{
	ebex_quat_t estimate;
	ebex_quat_t *corrected_quaternions;
	size_t i, j;

	if (!m_pop)
		die("Null pointer to population structure passed.");
	if (m_pop->size < 1)
		die("Pointer to empty population structure passed.");
	if (m_size > m_pop->size)
		die("Excessive size argument");

	ebex_quat_from_scalar(&estimate, 0.0);

#	pragma omp parallel for \
		shared (estimate, m_pop) \
		private(i) \
		schedule(static)
	for (i = 0; i < m_size; ++i)
	{
		estimate.x += fabs(m_pop->sorted_organisms[i]->chromosome[m_chromosome].q_allele[m_allele].x);
		estimate.y += fabs(m_pop->sorted_organisms[i]->chromosome[m_chromosome].q_allele[m_allele].y);
		estimate.z += fabs(m_pop->sorted_organisms[i]->chromosome[m_chromosome].q_allele[m_allele].z);
		estimate.w += fabs(m_pop->sorted_organisms[i]->chromosome[m_chromosome].q_allele[m_allele].w);
	}

	for (i = 1, j = 0; i < 3; i++)
	{
		if (estimate.element[i] >= estimate.element[j])
		{
			estimate.element[j] = 0.0;
			j = i;
		}
		else estimate.element[i] = 0.0;
	}
	estimate.element[j] = 1.0;

	/// Ensure that all quaternions are pointing in the same hemisphere
	corrected_quaternions = alloca(m_size * sizeof(ebex_quat_t));

#	pragma omp parallel for \
		shared (corrected_quaternions, m_pop) \
		private(i, estimate) \
		schedule(static)
	for (i = 0; i < m_size; i++)
	{
		if (ebex_quat_dot(&estimate, &(m_pop->sorted_organisms[i]->chromosome[m_chromosome].q_allele[m_allele])) >= 0.0)
			ebex_quat_copy(&corrected_quaternions[i], &(m_pop->sorted_organisms[i]->chromosome[m_chromosome].q_allele[m_allele]));
		else
			ebex_quat_neg(&corrected_quaternions[i], &(m_pop->sorted_organisms[i]->chromosome[m_chromosome].q_allele[m_allele]));
	}

	ebex_quat_copy(m_mean, &estimate);
	ebex_quat_spherical_weighted_mean_corrected(m_mean, corrected_quaternions, NULL, 1.0 - ebex_slerp_tol, 20, m_size);
}

double genetic_allele_mean_double(population_t *m_pop, size_t m_chromosome, size_t m_allele, size_t m_size)
{
	size_t i;
	double retval = 0.0;

	if (!m_pop)
		die("Null pointer to population structure passed.");
	if (m_pop->size < 1)
		die("Pointer to empty population structure passed.");
	if (m_size > m_pop->size)
		die("Excessive size argument");

	for (i = 0; i < m_size; i++) retval += m_pop->sorted_organisms[i]->chromosome[m_chromosome].dbl_allele[m_allele];

	return retval / (double)m_size;

}


bool genetic_fitness_statistics(population_t *m_pop, double *m_max, double *m_min, double *m_mean, double *m_median,
		double *m_var, double *m_stddev, double *m_kurtosis, double *m_skew)
{
	int i;
	double sum2 = 0.0, sum3 = 0.0, sum4 = 0.0;
	double mean = 0.0;
	double tmp = 0.0;

	if (!m_pop)
		die("Null pointer to population structure passed.");
	if (m_pop->size < 1)
		die("Pointer to empty population structure passed.");

	if(m_min) *m_min = m_pop->sorted_organisms[0]->fitness;
	if(m_max) *m_max = m_pop->sorted_organisms[m_pop->size - 1]->fitness;
	if(m_median) *m_median = *m_min + (*m_max - *m_min) / 2;


	for (i = 0; i < m_pop->size; i++)
	{
		tmp += m_pop->sorted_organisms[i]->fitness;
	}
	mean = tmp / (double)m_pop->size;

	if(m_mean) *m_mean = mean;


	for (i = 0; i < m_pop->size; i++)
	{
		tmp = m_pop->sorted_organisms[i]->fitness - mean;

		sum2 += tmp * tmp;
		sum3 += tmp * tmp * tmp;
		sum4 += tmp * tmp * tmp * tmp;
	}

	tmp = sum2 / m_pop->size;
	if (m_var) *m_var = tmp;
	if (m_skew) *m_skew = (sum3 / m_pop->size) / pow(tmp, 3.0 / 2.0);
	if (m_kurtosis) *m_kurtosis = (sum4 / m_pop->size) / (tmp * tmp);
	if (m_stddev) *m_stddev = sqrt(tmp);

	return true;
}

static inline void genetic_double_statistics(double *m_vector, size_t m_len, double *m_max, double *m_min, double *m_mean, double *m_median,
		double *m_var, double *m_stddev, double *m_kurtosis, double *m_skew)
{
	size_t i;
	double sum2 = 0.0, sum3 = 0.0, sum4 = 0.0;
	double mean = 0.0;
	double tmp = 0.0;

	sort_double(m_vector, m_len);

	if(m_min) *m_min = m_vector[0];
	if(m_max) *m_max = m_vector[m_len - 1];
	if(m_median) *m_median = *m_min + (*m_max - *m_min) / 2;


	for (i = 0; i < m_len; i++)
	{
		tmp += m_vector[i];
	}
	mean = tmp / (double)m_len;

	if(m_mean) *m_mean = mean;


	for (i = 0; i < m_len; i++)
	{
		tmp = m_vector[i] - mean;

		sum2 += tmp * tmp;
		sum3 += tmp * tmp * tmp;
		sum4 += tmp * tmp * tmp * tmp;
	}

	tmp = sum2 / (double)m_len;
	if (m_var) *m_var = tmp;
	if (m_skew) *m_skew = (sum3 / m_len) / pow(tmp, 3.0 / 2.0);
	if (m_kurtosis) *m_kurtosis = (sum4 / m_len) / (tmp * tmp);
	if (m_stddev) *m_stddev = sqrt(tmp);
}

bool genetic_allele_double_statistics(population_t *m_pop, int m_chromosome, int m_allele, double *m_max, double *m_min, double *m_mean, double *m_median,
		double *m_var, double *m_stddev, double *m_kurtosis, double *m_skew)
{
	int i;
	double *allele_array = NULL;


	if (!m_pop)
		die("Null pointer to population structure passed.");
	if (m_pop->size < 1)
		die("Pointer to empty population structure passed.");

	allele_array = e_malloc(m_pop->size * sizeof(double));

	for (i = 0; i < m_pop->size; i++) allele_array[i] = m_pop->sorted_organisms[i]->chromosome[m_chromosome].dbl_allele[m_allele];

	genetic_double_statistics(allele_array, m_pop->size, m_max, m_min, m_mean, m_median, m_var, m_stddev, m_kurtosis, m_skew);
	e_free(allele_array);
	return true;
}

bool genetic_allele_quaternion_statistics(population_t *m_pop, int m_chromosome, int m_allele, double *m_max, double *m_min, double *m_mean, double *m_median,
		double *m_var, double *m_stddev, double *m_kurtosis, double *m_skew)
{
	int i;
	double *allele_array = NULL;

	if (!m_pop)
		die("Null pointer to population structure passed.");
	if (m_pop->size < 1)
		die("Pointer to empty population structure passed.");

	allele_array = e_malloc(m_pop->size * sizeof(double));

	for (i = 0; i < m_pop->size; i++)
	{
		allele_array[i] = ebex_quat_get_roll(&m_pop->sorted_organisms[i]->chromosome[m_chromosome].q_allele[m_allele]);
	}
	genetic_double_statistics(allele_array, m_pop->size, m_max, m_min, m_mean, m_median, m_var, m_stddev, m_kurtosis, m_skew);

	for (i = 0; i < m_pop->size; i++)
	{
		allele_array[i] = ebex_quat_get_pitch(&m_pop->sorted_organisms[i]->chromosome[m_chromosome].q_allele[m_allele]);
	}
	if (m_max) m_max++;
	if (m_min) m_min++;
	if (m_mean) m_mean++;
	if (m_median) m_median++;
	if (m_var) m_var++;
	if (m_stddev) m_stddev++;
	if (m_kurtosis) m_kurtosis++;
	if (m_skew) m_skew++;
	genetic_double_statistics(allele_array, m_pop->size, m_max, m_min, m_mean, m_median, m_var, m_stddev, m_kurtosis, m_skew);

	for (i = 0; i < m_pop->size; i++)
	{
		allele_array[i] = ebex_quat_get_yaw(&m_pop->sorted_organisms[i]->chromosome[m_chromosome].q_allele[m_allele]);
	}
	if (m_max) m_max++;
	if (m_min) m_min++;
	if (m_mean) m_mean++;
	if (m_median) m_median++;
	if (m_var) m_var++;
	if (m_stddev) m_stddev++;
	if (m_kurtosis) m_kurtosis++;
	if (m_skew) m_skew++;
	genetic_double_statistics(allele_array, m_pop->size, m_max, m_min, m_mean, m_median, m_var, m_stddev, m_kurtosis, m_skew);

	e_free(allele_array);
	return true;
}
